import numpy as np
import pandas as pd
import math
import gurobipy as gp
from gurobipy import GRB
from Utilities import Instance, create_tour, create_routes


# tsp reformulated to accept specific routes to optimize
def tsp(cities, instance: Instance):
    # print("these are cities",cities)
    cities1zero = []
    for i in range(1, len(cities)):
        cities1zero.append(cities[i])
    cities0zero = []
    for i in range(1, len(cities) - 1):
        cities0zero.append(cities[i])
    m = gp.Model("Travelling Salesman Problem")

    # Create variables
    travel = {}
    z = {}
    for i in cities1zero:
        z[i] = m.addVar()
        for j in cities1zero:
            travel[i, j] = m.addVar(vtype=GRB.BINARY, obj=instance.d[i, j])

    # departure constraints
    for i in cities1zero:
        m.addConstr(sum(travel[i, j] for j in cities1zero) == 1)

    # arrival constraints
    for j in cities1zero:
        m.addConstr(sum(travel[i, j] for i in cities1zero) == 1)

    # Subtourelimination

    for i in cities0zero:
        for j in cities0zero:
            if i != j:
                m.addConstr(z[i] - z[j] + len(cities0zero) * travel[i, j] <= len(cities0zero) - 1)

    # m.Params.TimeLimit=600
    # m.Params.MIPGap=0.1
    m.optimize()

    print(m.objVal)

    for i in cities1zero:
        for j in cities1zero:
            print(travel[i, j].x, i, j, "i,j")

    tour = [0]
    y = 0
    # print("cities",len(cities))
    while len(tour) < len(cities):
        for i in cities:
            for j in cities:
                if travel[i, j].x > m.params.IntFeasTol and i == tour[y] and len(tour) < len(cities):
                    tour.append(j)
                    y += 1

    # global total
    # total += dist(result)

    m.dispose()

    return tour


# calculate distance
def capacity(d, instance: Instance):
    cap = 0
    for h in range(1, len(d) - 1):
        if d[h] > instance.C and len(d) > 4:
            return instance.Q + 1
        elif d[h] <= instance.C:
            cap += instance.nodes[d[h]]["demand"]
            # instance.q[d[h]]
        else:
            return 0
    return cap


# 返回配送路径 d 的成本（OD的补贴，RD的配送距离）
def dist(d, instance: Instance):
    # 如果是OD直接返回自己的目的地
    # 如果 d 这个点是 OD 的目的地点，且此路径只有这一个客户点
    if any(y > instance.C for y in d) and len(d) == 3:
        return 0

    # 如果是OD承担配送任务后返回目的地
    # 如果 d 这个点是 OD 的目的地点，且此路径有两个访问点
    # 为什么会出现四个访问点，论文中明明在说，OD车辆车辆只能服务一个客户点
    # 最后一个点是什么，未可知也，可能是0，仓库访问点
    # 这一小段的思想可以用于，我计算众包车辆的绕行距离上面
    if any(y > instance.C for y in d) and len(d) == 4:
        dis = 0
        # 计算总的距离
        for h in range(len(d) - 2):
            dis += instance.d_sd[d[h], d[h + 1]]
        # 减去原来的行驶距离，最后得到的是 绕行距离
        dis -= instance.d_sd[d[0], d[d.index(max(d))]]
        return dis * instance.rho

    # 如果非以上两种情况，则返回此距离（此距离可以作为配送的总成本）
    dis = 0
    for h in range(len(d) - 1):
        dis += instance.d_sd[d[h], d[h + 1]]
    return dis


# 返回此OD配送的总路径长度，d中的四个点为：0->k->l->0
def realdistOD(d, instance: Instance):
    if any(y > instance.C for y in d) and len(d) == 4:
        dis = 0
        # 因为 len(d)-1，指的是0
        for h in range(len(d) - 2):
            dis += instance.d_sd[d[h], d[h + 1]]
        return dis


# 使用 a b 路径，构建新的路径：0 ->...-> 0
def newtour(a, b):
    tour = []
    tour.append(0)
    for x in range(1, len(a) - 1):
        tour.append(a[x])
    for y in range(1, len(b) - 1):
        tour.append(b[y])
    tour.append(0)
    return tour


def swap(d, i, j):
    di = d[:]
    di[i] = d[j]
    di[j] = d[i]
    if ((j - i) >= 3):
        for k in range(int((j - i - 1) / 2)):
            di[i + k + 1] = d[j - k - 1]
            di[j - k - 1] = d[i + k + 1]
    return di


# start是一条路径
# 对 start这条路径中的访问点顺序再进行一次优化
def twoopt(start, instance: Instance):
    # print("Applying 2-opt(steepest descent) to", start, "...")
    x = 1
    mindist = dist(start, instance=instance)
    currentbest = start[:]
    count = 1

    # 对 start的访问数序进行邻域优化，直到没有改进空间为止
    while (count >= 1):
        count = 0
        for i in range(1, len(start) - 2):
            for j in range(1 + i, len(start) - 1):
                if dist(swap(start, i, j), instance=instance) < mindist:
                    mindist = dist(swap(start, i, j), instance=instance)
                    currentbest = swap(start, i, j)[:]
                    count += 1
        start = currentbest[:]

    # print("... The solution is:", start, " with a distance of", mindist)
    return start


def tourswap(j, k, x, y):
    dj = j[:]
    dk = k[:]
    dj.insert(j.index(x), y)
    # dj.insert(k.index(y), x)
    # dj.remove(x)
    dk.remove(y)
    return [dj, dk]


# 启发式算法
def heuristic(instance: Instance):
    customers = instance.C + instance.K  # 所有访问点的个数，包括客户访问点和 OD目的地点
    maxsaving = 1
    saveroute = {}

    # 方法(1)生成初始解：所有的客户访问点一开始都是 0->i->0，都由RD车辆配送
    # x就是路径集合。初始解包括：仓库出发到客户点再回到仓库，仓库出发到OD的目的地再回到仓库
    x = {}
    for j in range(customers):
        x[j] = [0, j + 1, 0]

    # 下面步骤对 x 进行操作，合并路径，直到不能减少路径优化空间为止
    while maxsaving > 0:
        maxsaving = 0
        # 先将 parent1 和 parent2 设置为不可能取到的值
        parent1 = customers + 1
        parent2 = customers + 1
        # 研究 将 k路径 和 l路径合并起来，遍历所有的点，找到一个节约成本最佳的方案。合并两条路径，生成新路径。
        for k in range(customers):
            for l in range(k, customers):
                # 此处要求，l与k必须有一个是客户点，另一个可以是客户点，也可以是OD目的地
                if k != l and k in x and l in x and (l <= instance.C - 1 or k <= instance.C - 1):

                    # 把k路径和l路径合并，节约的成本要大于 maxsaving；
                    # 新路径的容量不能超过 Q
                    # 路径中的访问点必须都是，客户点，不能有OD目的地
                    if (dist(x[k], instance=instance) + dist(x[l], instance=instance) \
                        - dist(newtour(x[k], x[l]), instance=instance)) > maxsaving \
                            and (capacity(newtour(x[k], x[l]), instance=instance)) <= instance.Q \
                            and (l <= instance.C - 1 and k <= instance.C - 1):

                        # 合并k和l，减少的距离成本
                        maxsaving = dist(x[l], instance=instance) + dist(x[k], instance=instance) - dist(
                            newtour(x[k], x[l]), instance=instance)
                        saveroute = newtour(x[k], x[l])
                        parent1 = k
                        parent2 = l

                    # 把k路径和l路径合并，节约的成本要大于 maxsaving；
                    # 路径容量限制；k为客户点，l为OD的目的地；
                    # OD配送，0->k->l->0
                    # 绕行距离在OD接受范围之内
                    elif (dist(x[k], instance=instance) + dist(x[l], instance=instance)
                          - dist(newtour(x[k], x[l]), instance=instance)) > maxsaving \
                            and (capacity(newtour(x[k], x[l]), instance=instance)) <= instance.Q \
                            and l > instance.C - 1 \
                            and len(newtour(x[k], x[l])) == 4 \
                            and (realdistOD(newtour(x[k], x[l]), instance=instance) - instance.d_sd[0, l + 1]) \
                            < instance.max_dev * instance.d_sd[0, l + 1]:

                        # 合并路径 k与l
                        maxsaving = dist(x[l], instance=instance) \
                                    + dist(x[k], instance=instance) \
                                    - dist(newtour(x[k], x[l]), instance=instance)
                        saveroute = newtour(x[k], x[l])
                        parent1 = k
                        parent2 = l

                    # 下面这个判断，大可不必，因为l必定要比k大，所以k为OD目的地的时候，l必定也是目的地，如此便出了问题
                    # 把k路径和l路径合并，节约的成本要大于 maxsaving；
                    # 合并后的路径容量限制；
                    # k为OD的目的地；
                    # OD配送；0->l->k->0
                    # 绕行距离在OD接受范围之内
                    elif (dist(x[k], instance=instance) + dist(x[l], instance=instance)
                          - dist(newtour(x[k], x[l]), instance=instance)) > maxsaving \
                            and (capacity(newtour(x[k], x[l]), instance=instance)) <= instance.Q \
                            and k > instance.C - 1 \
                            and len(newtour(x[k], x[l])) == 4 \
                            and (realdistOD(newtour(x[k], x[l]), instance=instance) - instance.d_sd[0, k + 1]) \
                            < instance.max_dev * instance.d_sd[0, k + 1]:

                        # 把k路径和l路径合并
                        maxsaving = dist(x[l], instance=instance) + dist(x[k], instance=instance) - dist(
                            newtour(x[k], x[l]), instance=instance)
                        saveroute = newtour(x[k], x[l])
                        parent1 = k
                        parent2 = l

        # 删除 组合成新路径的两个父路径
        if parent1 != customers + 1:
            del x[parent1]
            del x[parent2]
            x[parent1] = saveroute
        # print(maxsaving)
        # print(saveroute)
    print(x)


    # 对每条由RD配送的路径中访问点的顺序，进行重新优化。总的配送距离为 leng
    leng = 0
    for v in range(customers): # 对于每一个访问点，v是作为索引吧
        if v in x:
            # 如果此路径由RD配送
            if max(x[v]) <= instance.C:
                x[v] = twoopt(x[v], instance=instance)
            # print(dist(x[v],instance = instance))
            leng += dist(x[v], instance=instance)
    # print("The final tours are: ", x, "with a total length of: ", leng)

    # 将x复制到sol中
    sol = []
    for i in x:
        # if max(x[i])> instance.C:
        sol.append(x[i])

    """
    odtourss=[]
    for x in range(instance.K):
        best=np.inf
        bestcust=np.inf
        for y in range(1,instance.C+1):
            if instance.d_sd[y,instance.C+1+x]<best:
                best=instance.d_sd[y,instance.C+1+x]
                bestcust=y
        odtourss.append([0,bestcust,instance.C+1+x,0])
    

    
    for j in range(len(sol)):
            for k in range(j,len(sol)):
                if j != k and max(sol[j])<=instance.C and max(sol[k])<=instance.C:
                    for x in sol[j]:
                        for y in sol[k]:
                            if x !=0 and y!= 0:
                                #print(sol[k],sol[j])
                                sw=tourswap(sol[j],sol[k],x,y)
                                sw[0]=twoopt(sw[0],instance=instance)
                                sw[1] = twoopt(sw[1], instance=instance)
                                #print(sw)
                                if (dist(sol[j], instance = instance)+dist(sol[k], instance = instance))>(dist(sw[0], instance = instance)+dist(sw[1], instance = instance))and(capacity(sw[0],instance=instance)<= instance.Q) and (capacity(sw[1],instance=instance) <= instance.Q):
                                    print("better")
                                    sol[j]=sw[0][:]
                                    sol[k]=sw[1][:]
    for j in range(len(sol)):
            for k in range(j, len(sol)):
                if j != k and max(sol[j]) <= instance.C and max(sol[k]) <= instance.C:
                    for x in sol[j]:
                        for y in sol[k]:
                            if x != 0 and y != 0:
                                # print(sol[k],sol[j])
                                sw = tourswap(sol[k], sol[j], y, x)
                                sw[0] = twoopt(sw[0], instance=instance)
                                sw[1] = twoopt(sw[1], instance=instance)
                                # print(sw)
                                if (dist(sol[j], instance=instance) + dist(sol[k], instance=instance)) > (dist(sw[0], instance=instance) + dist(sw[1], instance=instance)) and (capacity(sw[0],instance=instance)<=instance.Q) and (capacity(sw[1],instance=instance)<=instance.Q):
                                    print("better")
                                    sol[j] = sw[0][:]
                                    sol[k] = sw[1][:]
    
    for v in range(len(sol)):
        if max(sol[v]) <= instance.C:
            sol[v]=tsp(sol[v], instance=instance)[:]
            print("tset", sol[v])
    
    RDtour=[0]
    for v in range(len(sol)):
        if max(sol[v]) <= instance.C:
            sol[v].pop(0)
            sol[v].pop(len(sol[v])-1)
            for x in range(len(sol[v])):
                RDtour.append(sol[v][x])
    RDtour.append(0)
    
    RDtour = [0]
    for i in range(1,instance.C+1):
        if not any(i in sublist for sublist in odtourss):

            RDtour.append(i)
    RDtour.append(0)
    

    y={}
    co=[]
    for x in range(len(RDtour)-1):
        for f in instance.nodes:
            if f["id"]==RDtour[x]:
                co.append([f["x"],f["y"]])
        print("co",co)


    print("RDtour",RDtour)
    customers = len(RDtour) - 2
    print("customers", customers)
    print("co",len(co))

    polar = [[0 for col in range(3)] for row in range(customers+1)]
    ci = [[0 for col in range(2)] for row in range(customers+1)]

    for i in range(1, customers + 1):
        co[i][0]=co[i][0]-35
        co[i][1]=co[i][1]-35
        polar[i][0] = co[i][0]
        polar[i][1] = co[i][1]



        if co[i][0] > 0 and co[i][1] > 0:
            polar[i][2] = math.degrees(math.atan(co[i][1] / co[i][0]))
        if co[i][0] > 0 and co[i][1] == 0:
            polar[i][2] = 0
        if co[i][0] < 0 and co[i][1] > 0:
            polar[i][2] = 180 + math.degrees(math.atan(co[i][1] / co[i][0]))
        if co[i][0] == 0 and co[i][1] > 0:
            polar[i][2] = 90
        if co[i][0] < 0 and co[i][1] < 0:
            polar[i][2] = 180 + math.degrees(math.atan(co[i][1] / co[i][0]))
        if co[i][0] < 0 and co[i][1] == 0:
            polar[i][2] = 180
        if co[i][0] > 0 and co[i][1] < 0:
            polar[i][2] = 360 + math.degrees(math.atan(co[i][1] / co[i][0]))
        if co[i][0] == 0 and co[i][1] < 0:
            polar[i][2] = 270

    polar.sort(key=lambda x: x[2])

    print("polar",polar)
    mintour=[]
    mincost=np.inf



    for i in range(1, customers+1):
            co[i][0] = co[i][0] + 35
            co[i][1] = co[i][1] + 35
            ci[i][0] = polar[i][0]+35
            ci[i][1] = polar[i][1]+35
    mintour = []
    mincost = np.inf
    for i in range(len(RDtour)):
        if i != 0:
            print("§§§§§§§§§§§§§§§§§§§",ci)
            ci.append(ci[1])
            ci.pop(1)
            print("§§§§§§§§§§§§§§§§§§§", ci)
        
        # compute distance matrix
    
        distance = np.zeros((customers + 1, customers + 1))
    
        for i in range(customers + 1):
            for j in range(customers + 1):
                if (i == j):
                    distance[i, j] = 10000
                else:
                    distance[i, j] = math.sqrt(
                        (ci[i][0] - ci[j][0]) * (ci[i][0] - ci[j][0]) + (ci[i][1] - ci[j][1]) * (ci[i][1] - ci[j][1]))
    
        
        def distsweep(d):
            dis = 0
            for h in range(len(d) - 1):
                dis += distance[d[h]][d[h + 1]]
            return dis
        
        def add(a, b):
            tour = []
            tour.append(0)
            for x in range(1, len(a) - 1):
                tour.append(a[x])
            tour.append(b)
            tour.append(0)
            return tour



        print("$$$$$$$$$$$$$$$$$",RDtour[co.index(ci[1])])
        print("$$$$$$$$$$$$$$$$$", RDtour[co.index(ci[2])])
        print("$$$$$$$$$$$$$$$$$", RDtour[co.index(ci[3])])
        print("$$$$$$$$$$$$$$$$$", RDtour[co.index(ci[4])])
        print("$$$$$$$$$$$$$$$$$", RDtour[co.index(ci[5])])

        print(ci[1])
        print(co.index(ci[1]))
        print(instance.nodes)
        print(RDtour)
        print(ci)
        print(co)

        j = 0
        y[0] = [0, RDtour[co.index(ci[1])], 0]
        for i in range(customers - 1):
            print(i, j, "i,j")
            if capacity(add(y[j], RDtour[co.index(ci[i+2])]),instance=instance) <= instance.Q:
                y[j] = add(y[j], RDtour[co.index(ci[i+2])])
            else:
                j += 1
                y[j] = [0, RDtour[co.index(ci[i+2])], 0]


        # Travelling salesman
        final = []
        for i in range(customers):

            # determine optimal solution

            if i in y:
                print(y[i])
                #final.append(tsp(y[i],instance=instance))
                final.append(twoopt(y[i], instance=instance))
                #final.append(y[i])

        cost = 0
        for v in range(len(final)):
            cost += dist(final[v], instance=instance)
        print(cost)
        print("Final routes: ", final, "Toutal cost: ", cost)

        for v in range(len(sol)):
            if max(sol[v]) > instance.C:
                final.append(sol[v])

        #for v in range(len(odtourss)):
        #    if max(odtourss[v]) > instance.C:
        #         final.append(odtourss[v])

        cost = 0
        for v in range(len(final)):
            cost += dist(final[v], instance=instance)
        print(cost)
        print("Final routes: ", final, "Toutal cost: ", cost)
        if cost < mincost:
            mincost=cost
            mintour=final[:]
    print("mintour", mincost,mintour)
    print(instance.nodes)

    for v in range(len(final)):
        if max(sol[v]) > instance.C:
            sol[v].pop(len(sol[v])-1)
    #for v in range(len(final)):
    #    if max(final[v]) > instance.C:
    #        final[v].pop(len(final[v])-1)

    #final=[[0, 6, 5, 17, 16, 14, 15, 2, 0], [0, 21, 22, 23, 4, 25, 24, 3, 9, 20, 10, 11, 19, 8, 18, 0], [0, 1, 27,0], [0, 7, 26,0], [0, 12, 29,0], [0, 13, 30,0]]
    # #final = [[0, 7, 51,0]]

    print("DDDDD",final)
    cost = 0
    for v in range(len(final)):
        cost += dist(final[v], instance=instance)
    print("test,test,test",cost)
    print("mincost",mincost)
    print(capacity([0, 1, 50, 3, 24, 29, 34, 35, 9, 20, 30, 32, 11, 10, 31, 27, 0],instance=instance))
    print(capacity([0, 18, 7, 19, 49, 36, 47, 46, 8, 45, 17, 38, 44, 16, 5, 6, 0],instance=instance))
    print(capacity([0, 12, 4, 25, 39, 23, 22, 41, 15, 43, 14, 37, 42, 2, 21, 40, 0],instance=instance))
    """

    # 计算 cost
    cost = 0
    for v in range(len(sol)):
        cost += dist(sol[v], instance=instance)
    # 计算 nrod
    nrod = 0
    for v in range(len(sol)):
        if max(sol[v]) > instance.C and len(sol[v]) == 4:
            nrod += 1
    # 计算 odcomp
    odcomp = 0
    for v in range(len(sol)):
        if max(sol[v]) > instance.C and len(sol[v]) == 4:
            odcomp += dist(sol[v], instance=instance)
    print("Final tours Savings", sol)
    print("Final cost Savings", cost)
    print("Final ODs used Savings", nrod)
    print("Final OD compensation Savings", odcomp)
    print("Final toll Savings", 0)

    # 剔除 OD 配送的路径中，结尾的那个0
    for v in range(len(sol)):
        if max(sol[v]) > instance.C:
            sol[v].pop(len(sol[v]) - 1)
    return sol